#!/bin/sh 
### BEGIN INIT INFO
# Provides:          roscore
# Default-Start:     2 3 4 5
# Default-Stop:      0 1 6
# X-Interactive:     true
# Short-Description: Start/stop roscore service
### END INIT INFO
#
# roscore		This init.d script is used to start roscore.

#Definimos las variables de entorno de ROS
export ROS_ROOT=/root/ros/ros
export PATH=$ROS_ROOT/bin:$PATH
export PYTHONPATH=$ROS_ROOT/core/roslib/src:$PYTHONPATH
if [ ! "$ROS_MASTER_URI" ] ; then export ROS_MASTER_URI=http://localhost:11311 ; fi
export ROS_PACKAGE_PATH=/root/ros/visualization_tutorials:/root/ros/visualization:/root/ros/visualization_common:/root/ros/vision_opencv:/root/ros/sound_drivers:/root/ros/slam_gmapping:/root/ros/simulator_stage:/root/ros/simulator_gazebo:/root/ros/robot_model:/root/ros/physics_ode:/root/ros/navigation:/root/ros/laser_pipeline:/root/ros/laser_drivers:/root/ros/joystick_drivers:/root/ros/joystick_drivers_tutorials:/root/ros/imu_drivers:/root/ros/image_pipeline:/root/ros/image_transport_plugins:/root/ros/image_common:/root/ros/geometry_tutorials:/root/ros/geometry:/root/ros/driver_common:/root/ros/diagnostics:/root/ros/common_tutorials:/root/ros/common:/root/ros/common_msgs:/root/ros/camera_drivers:/root/ros/roslisp_support:/root/ros/rx:/root/ros/ros_tutorials:/root/ros/ros_comm:/root/ros/ros_ui
export ROS_WORKSPACE=/root/ros

pidof_roscore() {
	ps -ef | grep python | grep roscore | grep -v grep | awk '{print $2}'
}

roscore_nodes_start(){
	rosrun irobot_create_2_1 driver.py &
	rosrun rosbridge rosbridge.py &
	#rosrun gscam gscam
}

roscore_nodes_stop(){
	rosnode kill /create &
	#rosnode kill /gscam
}

roscore_start(){
	roscore &
	roscore_nodes_start
}

roscore_stop() {
	PID=$(pidof_roscore) || true

	if [ "${PID}" ]; then
		#Primero paramos los nodos que esten corriendo
		roscore_nodes_stop

		# in this case it is everything nice and dandy
		# and we kill roscore
		kill -s 2 $PID
		return 0
	fi
	return 1
}

roscore_wait_stop() {
	# running ?
	PIDTMP=$(pidof_roscore) || true
	if kill -0 "${PIDTMP:-}" 2> /dev/null; then
	    PID=$PIDTMP
	fi

	roscore_stop

	# wait until really stopped
	if [ -n "${PID:-}" ]; then
		i=0
		while kill -0 "${PID:-}" 2> /dev/null;  do
        		if [ $i = '60' ]; then
        			break;
        	 	else
        			if [ $i = '0' ]; then
                			echo -n " ... waiting "
        			else
                	      		echo -n "."
        		 	fi
        			i=$(($i+1))
        			sleep 1
        	      fi
		 done
	fi
}

case $1 in
	start)
		echo "Starting ros master"
		
		roscore_start

		PID=pidof_roscore || true
		if [ -n "$PID" ]; then
                        exit 0
                else
                        exit 1
                fi
	;;
	stop)
		echo "Stopping ros master"
	
		if roscore_wait_stop; then
                        exit 0
                else
                        exit 1
                fi
	;;
	graceful | reload | force-reload)

	;;
	restart)
		
		PID=$(pidof_roscore) || true
		if ! roscore_wait_stop; then
			echo "Stopping ros master"
                        exit 1 || true
                fi

		roscore_start

		PID=pidof_roscore || true

		if [ -n "$PID" ]; then
			echo "ros master started"
                        exit 0
                else
                        exit 1
                fi
	;;
	status)
		PID=$(pidof_roscore) || true
		if [ -n "$PID" ]; then
			echo "roscore is running (pid $PID)."
			exit 0
		else
			echo "roscore is NOT running."
			exit 1
		fi
	;;
	*)
	
		echo "Usage: /etc/init.d/roscore {start|stop|restart|status}"
		exit 1
	;;
esac
